import rclpy 
from nav2_simple_commander.robot_navigator import BasicNavigator,TaskResult
from geometry_msgs.msg import Pose,PoseStamped
from tf2_ros import TransformListener,Buffer
from tf_transformations import euler_from_quaternion,quaternion_from_euler
from rclpy.duration import Duration
from autopatrol_interfaces.srv import SpeakText
import cv2
from cv_bridge import CvBridge
from sensor_msgs.msg import Image

class PatrolNode(BasicNavigator):
    def __init__(self,node_name = "patrol_node"):
        super().__init__(node_name)
        #导航相关定义
        self.declare_parameter("initial_point",[0.0,0.0,0.0])
        self.declare_parameter("target_points",[1.0,1.0,1.57])
        self.initial_point = self.get_parameter("initial_point").value
        self.target_points = self.get_parameter("target_points").value
        #实时TF
        self.buffer = Buffer()
        self.listener = TransformListener(self.buffer,self)
        #语音播报客户端
        self.speak_client = self.create_client(SpeakText,"speech_text")
        #订阅与保存相机相关定义
        self.declare_parameter("image_save_path","/home/wangdahua/chapter7/chapter7_ws/src/autopatrol_wrobot/image_recoder")
        self.image_save_path = self.get_parameter("image_save_path").value
        self.bridge = CvBridge()
        self.latest_image = None
        self.subscriber_image = self.create_subscription(Image,"/camera_sensor/image_raw",self.image_callback,10)
    
    def image_callback(self,msg):
        self.latest_image = msg

    def recodeImage(self):
        if self.latest_image is not None:
            pose = self.Get_current_pose()
            cv_image = self.bridge.imgmsg_to_cv2(self.latest_image)

            cv2.imwrite(f"{self.image_save_path}/image_{pose.translation.x:3.2f}_{pose.translation.y:3.2f}.png",cv_image)


    def speech_text(self,text):
        #调用语音服务
        while not self.speak_client.wait_for_service(timeout_sec = 2.0):
            self.get_logger().info("等待语音服务上线...")
        request = SpeakText.Request()
        request.text = text
        future = self.speak_client.call_async(request)
        rclpy.spin_until_future_complete(self,future)
        if future.result() is not None:
            result = future.result().result
            if result == True:
                self.get_logger().info(f"语音合成成功:{text}")
            else:
                self.get_logger().info(f"语音合成失败:{text}")
        else:
            self.get_logger().info(f"语音合成服务请求失败")

    def Get_pose_by_xyyaw(self,x,y,yaw):
        #通过xy and yaw合成PoseStamped
        pose = PoseStamped()
        pose.header.frame_id = "map"
        pose.pose.position.x = x
        pose.pose.position.y = y
        rotation_quat = quaternion_from_euler(0,0,yaw)
        pose.pose.orientation.x = rotation_quat[0]
        pose.pose.orientation.y = rotation_quat[1]
        pose.pose.orientation.z = rotation_quat[2]
        pose.pose.orientation.w = rotation_quat[3]
        return pose

    def Init_robot_pose(self):
        #初始化位置
        self.initial_point = self.get_parameter("initial_point").value
        self.setInitialPose(self.Get_pose_by_xyyaw(self.initial_point[0],self.initial_point[1],self.initial_point[2]))
        self.waitUntilNav2Active()

    def Get_target_points(self):
        #通过参数获取目标点集合
        points = []
        self.target_points = self.get_parameter("target_points").value
        self.get_logger().info(f"数量:{int(len(self.target_points) / 3)}")
        for index in range(int(len(self.target_points) / 3)):
            x = self.target_points[index * 3]
            y = self.target_points[index * 3 + 1]
            yaw = self.target_points[index * 3 + 2]
            points.append([x,y,yaw])
            self.get_logger().info(f"获取到目标点{index}->({x},{y},{yaw})")

        return points

    def Nav_to_points(self,goal_pose):
        #导航到目标点
        self.waitUntilNav2Active()
        self.goToPose(goal_pose)
        while not self.isTaskComplete():
            feedback = self.getFeedback()
            if feedback:
                self.get_logger().info(
                    f"预计:{Duration.from_msg(feedback.estimated_time_remaining).nanoseconds / 1e9}s后到达")
        result = self.getResult()
        if result == TaskResult.SUCCEEDED:
            self.get_logger().info("导航成功")
        elif result == TaskResult.CANCELED:
            self.get_logger().info("导航取消")
        elif result == TaskResult.FAILED:
            self.get_logger().info("导航失败")
        else:
            self.get_logger().info("导航结果未知")


    def Get_current_pose(self):
        #通过TF获取当前位姿
        while rclpy.ok():
            try:
                tf = self.buffer.lookup_transform("map","base_footprint",rclpy.time.Time(seconds = 0),rclpy.time.Duration(seconds = 1))
                transform = tf.transform
                rotation_euler = euler_from_quaternion([
                transform.rotation.x,
                transform.rotation.y,
                transform.rotation.z,
                transform.rotation.w])    
                self.get_logger().info(f"平移{transform.translation},旋转四元数{transform.rotation},旋转欧拉角{rotation_euler}")
                return transform
            except Exception as e:
                self.get_logger().info(f"不能获取坐标变换,原因:{str(e)}")


def main():
    rclpy.init()
    patrol_node = PatrolNode()
    patrol_node.speech_text(text = "initial position")
    patrol_node.Init_robot_pose()
    patrol_node.speech_text(text = "initial position complete")

    while rclpy.ok():
        points = patrol_node.Get_target_points()
        for point in points:
            x,y,yaw = point[0],point[1],point[2]
            target_point = patrol_node.Get_pose_by_xyyaw(x,y,yaw)
            patrol_node.speech_text(text = f"go to point({x},{y})")
            patrol_node.Nav_to_points(target_point)
            #记录图像
            patrol_node.speech_text(text = "ready to recoder image")
            patrol_node.recodeImage()
            patrol_node.speech_text(text = "finished the recoder!")

    rclpy.shutdown()